

import lejos.nxt.*;

public class Car 
{
    // Commands for the motors
    private static int forward = 2,
                             backward = 1,
                             stop     = 3;
    
    private static final boolean REVERSE_SETUP = false;
	                         
    private static MotorPort leftMotor = MotorPort.A;
    private static MotorPort rightMotor = MotorPort.C;
    
    private static int leftPower;
    private static int rightPower;
    private static int maxSpeed = 100;
    private static double difference;
	
    private Car()
    {	   
    	
    } 
   
    public static void stop() 
    {
	    leftMotor.controlMotor(0,stop);
	    rightMotor.controlMotor(0,stop);
	    
	    Car.leftPower = 0;
	    Car.rightPower = 0;
    }
   
    public static void forward(int leftPower, int rightPower)
    {
	    leftMotor.controlMotor(leftPower,forward);
	    rightMotor.controlMotor(rightPower,forward);
	    
	    Car.leftPower = leftPower;
	    Car.rightPower = rightPower;
    }
   
    public static void backward(int leftPower, int rightPower)
    {
	    leftMotor.controlMotor(leftPower,backward);
	    rightMotor.controlMotor(rightPower,backward);
	    
	    Car.leftPower = leftPower;
	    Car.rightPower = rightPower;
    }
    
    public static void drive(int leftPower, int rightPower)
    {
    	if(maxSpeed < leftPower || maxSpeed < rightPower)
    	{
    		if(leftPower < rightPower)
    		{
    			difference = rightPower/maxSpeed;
    		}
    		
    		else 
    		{
    			difference = leftPower/maxSpeed;
    		}
    		
    		leftPower /= difference;
    		rightPower /= difference;
    	}
    	
    	if(0 <= leftPower)
    	{
    		leftMotor.controlMotor(leftPower, forward);
    	}
    	
    	else
    	{
    		leftMotor.controlMotor(-leftPower, backward);
    	}
    	
    	if(0 <= rightPower)
    	{
    		rightMotor.controlMotor(rightPower, forward);
    	}
    	
    	else
    	{
    		rightMotor.controlMotor(-rightPower, backward);
    	}
    	
    	Car.leftPower = leftPower;
	    Car.rightPower = rightPower;
    }
    
    public static void drive(int leftPower, int rightPower, int stamina)
    {
    	drive((int)((double)(stamina/100)*leftPower), (int)((double)(stamina/100)*rightPower));
    }
    
    /**
     * 
     * @return the sum of the left and right power
     */
    public static int getSpeed()
    {
    	return (leftPower + rightPower)/2;
    }
    
    /**
     * 
     * @return the sum of the left and right power
     */
    public static void setMaxSpeed(int speed)
    {
    	if(0 <= speed && 100 <= speed)
    		maxSpeed = speed;
    	
    	else if(100 < speed)
    		maxSpeed = 100;
    	
    	else
    		maxSpeed = 0;
    }
}
